/*
@2024-11-20: 将PL_isMaster功能集成到
@2024-10-23: 基于单芯片相机的socket服务端程序多线程版适配修改后用于2X2拼接相机。该服务端程序运行于4个zynq
             ps端，作为后台程序1
@2025-08-11: 功能实现-在PS端进行FITS文件的直接写入，不经过转bin过程
@2025-08-12: 1. 优化速度-不做数据重排和大小端处理（操作转移到PL端）
             2. 优化速度-使用栈空间替代动态内存分配，提升性能
             3. 优化速度-使用指针操作替代数组索引计算，减少运算开销
@2025-08-13: 1. 优化速度-优化fits写入方案，减少函数调用和乘法运算
            2. 优化速度-设置缓冲区减少IO操作
            3. 优化速度-预计算常量减少运算操作
            4. 功能修改-修改每通道的行像元个数 1250->1264
@2025-08-14:功能增加-增加接收ICS传递的路径和文件名
@2025-08-15:1.功能修改-修改头文件写入方式，确保正确写入
            2.优化速度-增加预计算常量，减少乘法操作
@2025-08-18: 1. 功能修改-修改中间8通道数据选择（后1250个数据）
            2. 优化速度-修改头文件写入及80字节填充方式
            3. 优化速度-预计算写入通道偏移量，减少乘法运算
@2025-08-26 1. 功能修改-采用追赶法优化写入操作，提升写入速度
*/

#include "MephistoCAM.h"
#include <sys/mman.h>
#include <fcntl.h>
#include <unistd.h>
#include <arpa/inet.h>      // for inet_aton()
#include <netinet/in.h>     // for struct sockaddr_in
#include <stdlib.h>         // for atoi(), strtoll(), strtod()
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <errno.h>
#include <sys/time.h>
#include <unistd.h>  // for fsync

#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <sys/ioctl.h>
#include <linux/spi/spidev.h>
#include <sched.h>
#include <termios.h>
#include <poll.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <pthread.h>
#include <sys/select.h>
#include <time.h>
#include <limits.h>

uint32_t *mem_ptr;

// 统计计时（微秒累计）
// static double g_time_calc_offsets_us = 0.0;
// static double g_time_write_top_us = 0.0;
// static double g_time_write_bottom_us = 0.0;
// static double g_time_write_total_us = 0.0; 
// static double g_time_rearrange_us = 0.0;
// static double g_time_wait_ddr_us = 0.0;
static double g_time_fflush_us = 0.0;
static double g_time_header_us = 0.0;
static double g_time_close_us = 0.0;
static double g_time_while_us = 0.0;


/* ==================== 配置参数 ==================== */

//UDP通讯用
#define INDEX_CAMERA 3              //Z1 = 0, Z2 = 1, Z3 = 2
#define INDEX_PS 5                 // main = 0 , sub = 1~4
#define INDEX_FUNC 3                //heart beat = 0, command = 1
#ifndef BUF_SIZE
#define BUF_SIZE 1024
#endif
   

// 图像尺寸配置
#define ROWS 9400                    // 图像总行数
#define COLS 10000                   // 图像总列数
#define CHANNELS_PER_ROW 16          // 每行16个通道
#define PIXELS_PER_CHANNEL 1250     // 每个通道1250个像素（有效数据）
#define RAW_PIXELS_PER_CHANNEL 1264 // 每个通道1264个像素（原始数据，包含无效数据）
#define DATA_PER_READ 20000          // 每次读取的有效数据量 (16 * 1250)
#define RAW_DATA_PER_READ 20224      // 每次读取的原始数据量 (16 * 1264)
#define PIXELS_PER_CHANNEL_BYTES (PIXELS_PER_CHANNEL * sizeof(uint16_t))  // 1250 * sizeof(uint16_t)

// FITS文件标准定义
#define FITS_BLOCK_SIZE 2880         // FITS块大小（字节），必须是2880的倍数

// FITS数据类型定义
#define FITS_USHORT_IMG 16           // 16位无符号整数图像
#define FITS_SHORT_IMG 32            // 16位有符号整数图像
#define FITS_LONG_IMG 64             // 32位有符号整数图像
#define FITS_FLOAT_IMG -32           // 32位浮点图像
#define FITS_DOUBLE_IMG -64          // 64位浮点图像


// 预计算的通道偏移量数组
static const size_t g_src_offsets[16] = {
    // 前4通道：每个通道前1250个数据
    0 * RAW_PIXELS_PER_CHANNEL,      // ch=0: 0
    1 * RAW_PIXELS_PER_CHANNEL,      // ch=1: 1264
    2 * RAW_PIXELS_PER_CHANNEL,      // ch=2: 2528
    3 * RAW_PIXELS_PER_CHANNEL,      // ch=3: 3792
    
    // 中间8通道：每个通道后1250个数据
    4 * RAW_PIXELS_PER_CHANNEL + 14, // ch=4: 5070
    5 * RAW_PIXELS_PER_CHANNEL + 14, // ch=5: 6334
    6 * RAW_PIXELS_PER_CHANNEL + 14, // ch=6: 7598
    7 * RAW_PIXELS_PER_CHANNEL + 14, // ch=7: 8862
    8 * RAW_PIXELS_PER_CHANNEL, // ch=8: 10126
    9 * RAW_PIXELS_PER_CHANNEL, // ch=9: 11390
    10 * RAW_PIXELS_PER_CHANNEL,// ch=10: 12654
    11 * RAW_PIXELS_PER_CHANNEL,// ch=11: 13918
    
    // 后4通道：每个通道前1250个数据
    12 * RAW_PIXELS_PER_CHANNEL + 14,     // ch=12: 15168
    13 * RAW_PIXELS_PER_CHANNEL + 14,     // ch=13: 16432
    14 * RAW_PIXELS_PER_CHANNEL + 14,     // ch=14: 17696
    15 * RAW_PIXELS_PER_CHANNEL + 14     // ch=15: 18960
};

static const size_t g_dst_offsets[16] = {
    0 * PIXELS_PER_CHANNEL,          // ch=0: 0
    1 * PIXELS_PER_CHANNEL,          // ch=1: 1250
    2 * PIXELS_PER_CHANNEL,          // ch=2: 2500
    3 * PIXELS_PER_CHANNEL,          // ch=3: 3750
    4 * PIXELS_PER_CHANNEL,          // ch=4: 5000
    5 * PIXELS_PER_CHANNEL,          // ch=5: 6250
    6 * PIXELS_PER_CHANNEL,          // ch=6: 7500
    7 * PIXELS_PER_CHANNEL,          // ch=7: 8750
    8 * PIXELS_PER_CHANNEL,          // ch=8: 10000
    9 * PIXELS_PER_CHANNEL,          // ch=9: 11250
    10 * PIXELS_PER_CHANNEL,         // ch=10: 12500
    11 * PIXELS_PER_CHANNEL,         // ch=11: 13750
    12 * PIXELS_PER_CHANNEL, // ch=12: 15000
    13 * PIXELS_PER_CHANNEL, // ch=13: 16250
    14 * PIXELS_PER_CHANNEL, // ch=14: 17500
    15 * PIXELS_PER_CHANNEL  // ch=15: 18750
};

// 预计算的FITS文件偏移量常量（使用编译期宏避免非整型常量初始化报错）
#define FITS_DATA_START (FITS_BLOCK_SIZE)
#define FITS_ROW_SIZE ((COLS) * (long)sizeof(uint16_t))
#define FITS_MAX_ROW_OFFSET (((ROWS) - 1) * (FITS_ROW_SIZE))


/* ==================== 数据结构定义 ==================== */

/**
 * FITS文件句柄结构体
 * 模拟CFITSIO的fitsfile结构，用于管理FITS文件操作
 */
typedef struct {
    FILE *fp;           // 文件指针
    int status;         // 操作状态码（0表示成功，非0表示错误）
    long naxes[2];      // 图像维度数组 [列数, 行数]
    int bitpix;         // 数据类型标识（16表示uint16）
    int naxis;          // 图像维度数量（2表示2D图像）
} fitsfile_embedded;

/* ==================== FITS文件操作函数 ==================== */

/**
 * 创建FITS文件

 */
int fits_create_file_embedded(fitsfile_embedded *fptr, const char *filename) {
    fptr->fp = fopen(filename, "wb");  // 以二进制写模式打开
    if (!fptr->fp) {
        return -1;
    }
    
    // 设置适中的文件缓冲区（64KB）以提升I/O性能，避免过大缓冲区导致的问题
    // char *buffer = malloc(512 * 1024);  // 512KB缓冲区
    // if (buffer) {
    //     if (setvbuf(fptr->fp, buffer, _IOFBF, 512 * 1024) != 0) {
    //         // 如果设置缓冲区失败，释放内存但不影响文件操作
    //         free(buffer);
    //     }
    // }
    
    fptr->status = 0;
    return 0;
}

/**
 * 写入单个FITS头记录（80字节）
 */
int write_fits_card(FILE *fp, const char *keyword, const char *value, const char *comment) {
    char card[81];  // 81字节，确保有空间存储字符串结束符
    
    // 优化：使用更高效的字符串格式化
    if (comment && strlen(comment) > 0) {
        // 有注释的情况
        snprintf(card, sizeof(card), "%-8s= %-20s / %s", keyword, value, comment);
    } else {
        // 无注释的情况
        snprintf(card, sizeof(card), "%-8s= %-20s", keyword, value);
    }
    
    // 优化：使用memset一次性填充空格，避免多次strcat
    size_t card_len = strlen(card);
    if (card_len < 80) {
        memset(card + card_len, ' ', 80 - card_len);
    }
    card[80] = '\0';  // 确保字符串结束符
    
    // 写入80字节（不包括字符串结束符）
    if (fwrite(card, 1, 80, fp) != 80) {
        return -1;
    }
    
    return 0;
}

/**
 * 写入整数类型的FITS头记录（80字节）
 */
int write_fits_card_int(FILE *fp, const char *keyword, int value, const char *comment) {
    char card[81];  // 81字节，确保有空间存储字符串结束符
    
    // 优化：使用更高效的字符串格式化
    if (comment && strlen(comment) > 0) {
        // 有注释的情况，整数右对齐
        snprintf(card, sizeof(card), "%-8s= %20d / %s", keyword, value, comment);
    } else {
        // 无注释的情况，整数右对齐
        snprintf(card, sizeof(card), "%-8s= %20d", keyword, value);
    }
    
    // 优化：使用memset一次性填充空格
    size_t card_len = strlen(card);
    if (card_len < 80) {
        memset(card + card_len, ' ', 80 - card_len);
    }
    card[80] = '\0';  // 确保字符串结束符
    
    // 写入80字节（不包括字符串结束符）
    if (fwrite(card, 1, 80, fp) != 80) {
        return -1;
    }
    
    return 0;
}

/**
 * 写入长整数类型的FITS头记录（80字节）
 */
int write_fits_card_long(FILE *fp, const char *keyword, long value, const char *comment) {
    char card[81];  // 81字节，确保有空间存储字符串结束符
    
    // 优化：使用更高效的字符串格式化
    if (comment && strlen(comment) > 0) {
        // 有注释的情况，长整数右对齐
        snprintf(card, sizeof(card), "%-8s= %20ld / %s", keyword, value, comment);
    } else {
        // 无注释的情况，长整数右对齐
        snprintf(card, sizeof(card), "%-8s= %20ld", keyword, value);
    }
    
    // 优化：使用memset一次性填充空格
    size_t card_len = strlen(card);
    if (card_len < 80) {
        memset(card + card_len, ' ', 80 - card_len);
    }
    card[80] = '\0';  // 确保字符串结束符
    
    // 写入80字节（不包括字符串结束符）
    if (fwrite(card, 1, 80, fp) != 80) {
        return -1;
    }
    
    return 0;
}

/**
 * 创建FITS图像并写入头文件信息
 * 
 * 写入完整的FITS头信息，包括必需的关键字
 * 
 * @param fptr FITS文件句柄指针
 * @param bitpix 数据类型标识
 * @param naxis 维度数量
 * @param naxes 各维度大小的数组
 * @return 0表示成功，-1表示失败
 */
int fits_create_img_embedded(fitsfile_embedded *fptr, int bitpix, int naxis, long *naxes) {
    if (!fptr->fp) {
        return -1;
    }
    
    struct timeval t_hdr_s, t_hdr_e;
    gettimeofday(&t_hdr_s, NULL);
    // 保存图像信息到结构体
    // fptr->bitpix = bitpix;
    // fptr->naxis = naxis;
    // memcpy(fptr->naxes, naxes, naxis * sizeof(long));
    
    // 写入最基本的FITS头记录（必需的最小集合）
    
    // SIMPLE = T (必需)
    if (write_fits_card(fptr->fp, "SIMPLE", "T", "file does conform to FITS standard") != 0) {
        return -1;
    }
    
    // BITPIX (必需) - 整数类型
    if (write_fits_card_int(fptr->fp, "BITPIX", bitpix, "number of bits per data pixel") != 0) {
        return -1;
    }
    // 强制无符号类型声明（防止DS9重新解释）
    if (write_fits_card_long(fptr->fp, "BZERO", 0, "offset data range to that of unsigned short") != 0) {
        return -1;
    }
    
    if (write_fits_card(fptr->fp, "BSCALE", "1", "default scaling factor") != 0) {
        return -1;
    }
    
    if (write_fits_card(fptr->fp, "DATATYPE", "UINT16", "data type specification") != 0) {
        return -1;
    }
    
    if (write_fits_card(fptr->fp, "SIGNED", "F", "data is unsigned") != 0) {
        return -1;
    }
    // NAXIS (必需) - 整数类型
    if (write_fits_card_int(fptr->fp, "NAXIS", naxis, "number of data axes") != 0) {
        return -1;
    }
    
    // NAXIS1, NAXIS2 (必需) - 长整数类型
    if (write_fits_card_long(fptr->fp, "NAXIS1", naxes[0], "length of data axis 1") != 0) {
        return -1;
    }
    
    if (write_fits_card_long(fptr->fp, "NAXIS2", naxes[1], "length of data axis 2") != 0) {
        return -1;
    }
    
    // END (必需)
    char end_card[81];
    memset(end_card, ' ', 80);  // 用空格填充，不是空字符
    end_card[80] = '\0';        // 字符串结束符
    memcpy(end_card, "END", 3); // 写入END关键字
    
    if (fwrite(end_card, 1, 80, fptr->fp) != 80) {
        return -1;
    }

    // 强制刷新缓冲区，确保头文件立即写入磁盘
    fflush(fptr->fp);
    
    // 填充到2880字节边界
    long current_pos = ftell(fptr->fp);
    long padding = FITS_BLOCK_SIZE - (current_pos % FITS_BLOCK_SIZE);
    
    if (padding < FITS_BLOCK_SIZE) {
        char padding_bytes[FITS_BLOCK_SIZE];
        memset(padding_bytes, ' ', FITS_BLOCK_SIZE);  // 用空格填充，不是空字符
        if (fwrite(padding_bytes, 1, (size_t)padding, fptr->fp) != (size_t)padding) {
            return -1;
        }
        // 再次刷新，确保填充数据也写入磁盘
        // fflush(fptr->fp);
    }
    gettimeofday(&t_hdr_e, NULL);
    g_time_header_us += (t_hdr_e.tv_sec - t_hdr_s.tv_sec) * 1e6 + (t_hdr_e.tv_usec - t_hdr_s.tv_usec);
    
    return 0;
}

char timestamp[26];
void gen_timestamp(char gmt_str[26]){
    struct timeval tv;    
    struct tm* st;
    gettimeofday(&tv, NULL);    
    
    st = gmtime(&tv.tv_sec);
    // sprintf(gmt_str, "cl%4d-%02d-%02dT%02d:%02d:%06.3f",st->tm_year+1900, st->tm_mon, st->tm_mday, st->tm_hour,st->tm_min,st->tm_sec+tv.tv_usec*1e-6);
    sprintf(gmt_str, "%4d-%02d-%02dT%02d:%02d:%06.3f",st->tm_year+1900, st->tm_mon+1, st->tm_mday, st->tm_hour,st->tm_min,st->tm_sec+tv.tv_usec*1e-6);
}

/**
 * 批量写入两行数据（对称写入、内存预整理）
 * 
 * 每次读取16*1264个数据，但只需要每个通道前1250个数据
 * 使用栈上缓冲区预整理数据，保持最优的写入性能
 * 前8个通道的前1250个数据写入上行，后8个通道的前1250个数据写入下行
 * 
 * @param fptr FITS文件句柄指针
 * @param row_cnt 当前行计数（从0开始）
 * @param raw_data 包含16*1264个原始数据的指针
 * @return 0表示成功，-1表示失败
 */
int fits_write_two_rows_embedded(fitsfile_embedded *fptr, int row_cnt, uint16_t *raw_data) {
    struct timeval t_total_s, t_total_e;
    gettimeofday(&t_total_s, NULL);
    if (!fptr->fp) {
        return -1;
    }
    
    // 直接使用预计算的常量，无需重复计算
    // if (row_cnt % 235 == 0) {
    //     gen_timestamp(timestamp);
    //     fprintf(stderr, "[%s]> start writing row %d\n", timestamp, row_cnt);
    // }

    // === 统计：计算偏移量（对应第400-405行） ===
    // struct timeval t_off_s, t_off_e;
    // gettimeofday(&t_off_s, NULL);
    long row_offset = (long)row_cnt * FITS_ROW_SIZE;
    
    // 第一行：第row_cnt行（从上往下）- 使用前8个通道的数据
    long row1_offset = FITS_DATA_START + row_offset;
    
    // 第二行：第ROWS-row_cnt行（从下往上）- 使用后8个通道的数据
    long row2_offset = FITS_DATA_START + FITS_MAX_ROW_OFFSET - row_offset;
    // gettimeofday(&t_off_e, NULL);
    // g_time_calc_offsets_us += (t_off_e.tv_sec - t_off_s.tv_sec) * 1e6 + (t_off_e.tv_usec - t_off_s.tv_usec);
    
    // 使用栈上缓冲区预整理数据（40KB：前10000个 + 后10000个）
    uint16_t extracted_data[20000];
    
    // 统计：数据重排/拷贝耗时
    // struct timeval t_re_s, t_re_e;
    // gettimeofday(&t_re_s, NULL);
    // 优化版本：预计算偏移量
    for (int ch = 0; ch < 16; ch++) {
        uint16_t *src = raw_data + g_src_offsets[ch];  // 源地址：每个通道的起始位置
        uint16_t *dst = extracted_data + g_dst_offsets[ch];  // 目标地址：连续存储
        memcpy(dst, src, PIXELS_PER_CHANNEL_BYTES);  // 直接拷贝1250个数据
    }
    // gettimeofday(&t_re_e, NULL);
    // g_time_rearrange_us += (t_re_e.tv_sec - t_re_s.tv_sec) * 1e6 + (t_re_e.tv_usec - t_re_s.tv_usec);
 
    // 字节序转换：直接对要写入的数组进行转换
    // for (int i = 0; i < 20000; i++) {
    //     extracted_data[i] = (extracted_data[i] >> 8) | (extracted_data[i] << 8);
    // }

    // === 统计：写入上半部分（对应第412-418行） ===
    // struct timeval t_top_s, t_top_e;
    // gettimeofday(&t_top_s, NULL);
    // 写入第一行（前10000个数据）
    if (fseek(fptr->fp, row1_offset, SEEK_SET) != 0) {
        return -1;
    }
    if (fwrite(extracted_data, sizeof(uint16_t), 10000, fptr->fp) != 10000) {
        return -1;
    }
    // gettimeofday(&t_top_e, NULL);
    // g_time_write_top_us += (t_top_e.tv_sec - t_top_s.tv_sec) * 1e6 + (t_top_e.tv_usec - t_top_s.tv_usec);
    
    // === 统计：写入下半部分（对应第421-426行） ===
    // struct timeval t_bot_s, t_bot_e;
    // gettimeofday(&t_bot_s, NULL);
    // 写入第二行（后10000个数据）
    if (fseek(fptr->fp, row2_offset, SEEK_SET) != 0) {
        return -1;
    }
    if (fwrite(extracted_data + 10000, sizeof(uint16_t), 10000, fptr->fp) != 10000) {
        return -1;
    }
    // gettimeofday(&t_bot_e, NULL);
    // g_time_write_bottom_us += (t_bot_e.tv_sec - t_bot_s.tv_sec) * 1e6 + (t_bot_e.tv_usec - t_bot_s.tv_usec);
    
    // 移除循环中的fflush，避免阻塞主循环
    // 只在文件关闭时进行同步操作
    if (row_cnt % 200 == 0) {
        fflush(fptr->fp);
    }

    // if (row_cnt % 235 == 0) {
    //     gen_timestamp(timestamp);
    //     fprintf(stderr, "[%s]> finished writing row %d\n", timestamp, row_cnt);
    // }
    // gettimeofday(&t_total_e, NULL);
    // g_time_write_total_us += (t_total_e.tv_sec - t_total_s.tv_sec) * 1e6 + (t_total_e.tv_usec - t_total_s.tv_usec);
    return 0;
}


// 关闭FITS文件
int fits_close_file_embedded(fitsfile_embedded *fptr) {
    struct timeval tcls, tcle;
    gettimeofday(&tcls, NULL);
    if (fptr->fp) {
        fflush(fptr->fp);
        fsync(fileno(fptr->fp)); // 确保数据写入磁盘
        fclose(fptr->fp);
        fptr->fp = NULL;
    }
    gettimeofday(&tcle, NULL);
    g_time_close_us += (tcle.tv_sec - tcls.tv_sec) * 1e6 + (tcle.tv_usec - tcls.tv_usec);
    return 0;
}


#pragma region UDP通讯变量初始化
    //相机状态
int ccd_id = 1;
int status_ccd = 1;

char fits_name[256] = { 0 }; //保存fits的名称
char fits_path[512] = { 0 }; //保存路径

const char* ip_table[INDEX_CAMERA] = { "192.168.0.100", "192.168.0.110", "192.168.0.120" };


static const uint16_t port_suffix[INDEX_FUNC][INDEX_PS] = {
    {11000, 13000, 13100, 13200, 13300}, // heartbeat
    {11001, 13001, 13101, 13201, 13301}, // command(readout_done for sub_ps)
    {11001, 13002, 13102, 13202, 13302} // command(switch_rate for sub_ps)
};

struct cmd_args {
    char cmd[128];
    struct sockaddr_in client_addr;
    int sockfd;
};

int ret;

char len_input[10]; // 用于存储用户输入的读取长度
int read_len;


// 心跳目标客户端列表（由 target_mutex 保护）
#define MAX_TARGETS 100
struct sockaddr_in target_list[MAX_TARGETS];
int target_count = 0;
pthread_mutex_t target_mutex;

// 活动命令处理线程列表（供 abort 时取消，用 thread_mutex 保护）
#define MAX_CMD_THREADS 100
pthread_t cmd_threads[MAX_CMD_THREADS];
int cmd_thread_count = 0;
pthread_mutex_t thread_mutex;
pthread_mutex_t status_mutex = PTHREAD_MUTEX_INITIALIZER;

typedef struct {
    int sockfd;
    char buffer[BUF_SIZE];
    struct sockaddr_in client_addr;
    socklen_t client_addr_len;
} request_t;


#pragma endregion

#pragma region UDP通讯用函数

// 将线程添加到活动线程列表（调用前需加锁 thread_mutex）1
void add_thread_to_list(pthread_t tid) {
    if (cmd_thread_count < MAX_CMD_THREADS) {
        cmd_threads[cmd_thread_count++] = tid;
    }
    else {
        fprintf(stderr, "Warning: command thread list full, cannot track more threads.\n");
    }
}

// 从活动线程列表移除线程（调用前需加锁 thread_mutex）
void remove_thread_from_list(pthread_t tid) {
    // 在列表中查找线程 ID
    for (int i = 0; i < cmd_thread_count; ++i) {
        if (pthread_equal(cmd_threads[i], tid)) {
            // 找到后将其后的元素前移覆盖
            for (int j = i; j < cmd_thread_count - 1; ++j) {
                cmd_threads[j] = cmd_threads[j + 1];
            }
            cmd_thread_count--;
            break;
        }
    }
}

// 线程清理处理函数：在线程退出或被取消时，将其从活动列表移除
void cleanup_remove_thread(void* arg) {
    // 即将退出的线程调用该清理函数
    pthread_t self_tid = pthread_self();
    pthread_mutex_lock(&thread_mutex);
    remove_thread_from_list(self_tid);
    pthread_mutex_unlock(&thread_mutex);
}

// 新增心跳目标客户端（注册客户端 IP 和端口）
void add_target(const char* ip, uint16_t port) {
    struct sockaddr_in addr;
    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(port);

    if (inet_aton(ip, &addr.sin_addr) == 0) {
        fprintf(stderr, "Invalid IP address format: %s\n", ip);
        return;
    }

    pthread_mutex_lock(&target_mutex);
    for (int i = 0; i < target_count; ++i) {
        if (addr.sin_addr.s_addr == target_list[i].sin_addr.s_addr &&
            addr.sin_port == target_list[i].sin_port) {
            pthread_mutex_unlock(&target_mutex);
            return;
        }
    }

    if (target_count < MAX_TARGETS) {
        target_list[target_count++] = addr;

        printf("Added heartbeat target %s:%d (index=%d)\n",
            inet_ntoa(addr.sin_addr), ntohs(addr.sin_port), target_count - 1);
    }
    else {
        fprintf(stderr, "Target list full, cannot add.\n");
    }

    pthread_mutex_unlock(&target_mutex);
}

// 心跳广播线程函数：每 500ms 向所有登记的客户端发送状态
void* heartbeat_thread_func(void* arg) {
    int sockfd;


    if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
        perror("socket for heartbeat");
        return NULL;
    }
    // 未显式绑定端口，使用内核分配的临时端口发送
    char msg[128];
    while (1) {
        // 构造状态消息
        pthread_mutex_lock(&status_mutex);
        int id = ccd_id;
        int ready = status_ccd;
        pthread_mutex_unlock(&status_mutex);
        // 格式: ccd_status:ID=z<id>$ready=<ready>
        snprintf(msg, sizeof(msg), "ccd_status:ID=%d$ready=%d",
            id, ready);
        // 向所有登记的目标发送
        pthread_mutex_lock(&target_mutex);
        for (int i = 0; i < target_count; ++i) {
            sendto(sockfd, msg, strlen(msg), 0,
                (struct sockaddr*)&target_list[i], sizeof(target_list[i]));
        }
        pthread_mutex_unlock(&target_mutex);
        // 每 500 毫秒发送一次心跳
        usleep(500000);
    }
    // 不会执行到此，但如需退出可关闭 socket
    close(sockfd);
    return NULL;
}

static void send_to_targets_on_port(int sockfd, const char* buf, size_t len, uint16_t fixed_port) {
    pthread_mutex_lock(&target_mutex);
    for (int i = 0; i < target_count; ++i) {
        struct sockaddr_in dst = target_list[i]; // 拷贝，不改全局
        dst.sin_port = htons(fixed_port);        // 覆盖端口为固定端口

        // 打印发送信息
        char ip_str[INET_ADDRSTRLEN];
        inet_ntop(AF_INET, &(dst.sin_addr), ip_str, sizeof(ip_str));
        fprintf(stderr, "Sending to %s:%u, message: %.*s\n",
                ip_str,
                fixed_port,
                (int)len, buf);

        sendto(sockfd, buf, len, 0, (struct sockaddr*)&dst, sizeof(dst));
    }
    pthread_mutex_unlock(&target_mutex);
}

static void send_to_targets(int sockfd, const char* buf, size_t len) {
    pthread_mutex_lock(&target_mutex);
    for (int i = 0; i < target_count; ++i) {
        sendto(sockfd,
            buf, len, 0,
            (struct sockaddr*)&target_list[i],
            sizeof(target_list[i]));
    }
    pthread_mutex_unlock(&target_mutex);
}

//指令执行函数，包含速度切换与link2camera
void* command_handler_thread_func(void* arg) {
    int id = ccd_id;
    struct cmd_args* args = arg;
    char* buffer = args->cmd;
    struct sockaddr_in client_addr = args->client_addr;
    int sockfd = args->sockfd;
    socklen_t client_addr_len = sizeof(client_addr);

    pthread_cleanup_push(cleanup_remove_thread, NULL);

    // 拆分命令和负载
    char* cmd, * payload;
    char* colon = strchr(buffer, ':');
    if (colon) {
        *colon = '\0';
        cmd = buffer;
        payload = colon + 1;
    }
    else {
        cmd = buffer;
        payload = NULL;
    }

    if (strncmp(cmd, "link_to_camera", 14) == 0 && payload) {
        char ip[64] = { 0 };
        int port = 0;

        char* ip_eq = strstr(payload, "ip=");
        char* port_eq = strstr(payload, "$port=");
        if (ip_eq && port_eq) {
            sscanf(ip_eq, "ip=%63[^$]", ip);
            port = atoi(port_eq + 6);
            printf("[DEBUG] Parsed %s:%d\n", ip, port); fflush(stdout);


            add_target(ip, port);


            send_to_targets(sockfd, "OK: target added", strlen("OK: target added"));
        }
        else {
            printf("Malformed link_to_camera payload: %s\n", payload);
        }
    }

    else if (strncmp(cmd, "get_frame_290_subps", 19) == 0 && payload) {
        char* fn_start = strstr(payload, "$filename=");
        if (fn_start) {
            fn_start += strlen("$filename="); // 跳过"$filename="
            char* fn_end = strchr(fn_start, '$'); // 找到下一个'$'作为结束
            if (!fn_end) fn_end = fn_start + strlen(fn_start); // 如果没有$, 取到末尾
            size_t len = fn_end - fn_start;
            if (len >= sizeof(fits_name)) len = sizeof(fits_name) - 1;
            strncpy(fits_name, fn_start, len);
            fits_name[len] = '\0';
        }

        // 找到 $path= 起始位置
        char* path_start = strstr(payload, "$path=");
        if (path_start) {
            path_start += strlen("$path=");
            char* path_end = strchr(path_start, '$'); // 可选，防止后面还有别的字段
            if (!path_end) path_end = path_start + strlen(path_start);
            size_t len = path_end - path_start;
            if (len >= sizeof(fits_path)) len = sizeof(fits_path) - 1;
            strncpy(fits_path, path_start, len);
            fits_path[len] = '\0';
        }

        fprintf(stderr, "filename = %s\n", fits_name);
        fprintf(stderr, "path = %s\n", fits_path);

       



        char sendbuf[BUF_SIZE];
        int sendlen = snprintf(sendbuf, sizeof(sendbuf),
            "get_frame_290_subps:$info=ok$filename=%s$fits_path=%s",
            fits_name, fits_path);
        if (sendlen < 0 || sendlen >= (int)sizeof(sendbuf)) {
            fprintf(stderr, "send buffer overflow or encoding error\n");
        }
        else {
            // 3. 发回给客户端
            send_to_targets_on_port(sockfd, sendbuf, sendlen, port_suffix[2][ccd_id + 1]);
        }
    
    }

    else if (strcmp(cmd, "readout_rate_290") == 0) {
        // payload 示例: "rate=1"
        if (payload) {
            char* eq = strchr(payload, '=');
            if (eq) {
                *eq = '\0';
                char* key = payload;
                char* value = eq + 1;
                if (strcmp(key, "$rate") == 0) {
                    char* endptr = NULL;
                    long rate = strtol(value, &endptr, 10);
                    if (endptr == value || *endptr != '\0') {
                        fprintf(stderr, "Invalid rate value: '%s'\n", value);
                    }
                    else {
                        switch (rate) {
                        case 0:
                            printf("readout_rate_290:set to 2m\n");
                            ret = system("/bin/bash /mnt/mosaic_up_2M.sh");
                            if (ret != 0) {
                                fprintf(stderr, "Failed to execute mosaic_up_2M.sh, return code: %d\n", ret);
                            }
                            break;




                        case 1:
                            printf("readout_rate_290:set to 1m\n");
                            ret = system("/bin/bash /mnt/mosaic_up_1M.sh");
                            if (ret != 0) {
                                fprintf(stderr, "Failed to execute mosaic_up_1M.sh, return code: %d\n", ret);
                            }


                            break;

                        case 2:
                            printf("readout_rate_290:set to 512k\n");
                            ret = system("/bin/bash /mnt/mosaic_up_512k.sh");
                            if (ret != 0) {
                                fprintf(stderr, "Failed to execute mosaic_up_512k.sh, return code: %d\n", ret);
                            }

                            break;

                        case 3:
                            printf("readout_rate_290:set to 256k\n");
                            ret = system("/bin/bash /mnt/mosaic_up_256k.sh");
                            if (ret != 0) {
                                fprintf(stderr, "Failed to execute mosaic_up_256k.sh, return code: %d\n", ret);
                            }

                            break;


                        default:
                            fprintf(stderr, "unsupported rate");
                            break;
                        }
                        usleep(500000);
                        char sendbuf[BUF_SIZE];
                        int sendlen = snprintf(sendbuf, sizeof(sendbuf),
                            "readout_rate_290=$info=ok$rate=%d$ID=%d",
                            rate, id);
                        if (sendlen < 0 || sendlen >= (int)sizeof(sendbuf)) {
                            fprintf(stderr, "send buffer overflow or encoding error\n");
                        }
                        else {
                            // 3. 发回给客户端
                            send_to_targets_on_port(sockfd, sendbuf, sendlen, port_suffix[2][ccd_id + 1]);


                        }
                    }

                }
            }
        }
    }


    else {
        printf("Unknown command: %s\n", buffer);
    }

    free(arg);
    pthread_cleanup_pop(1);
    return NULL;
}

// UDP 指令监听线程函数
void* command_listener_thread_func(void* arg) {
    int sockfd;
    struct sockaddr_in servaddr;
    if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
        perror("socket creation failed");
        return NULL;
    }
    // 允许端口快速重用
    int optval = 1;
    setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
    // 绑定 UDP 端口 10001 用于接收上位机指令
    memset(&servaddr, 0, sizeof(servaddr));
    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port = htons(10001);
    if (bind(sockfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) < 0) {
        perror("bind failed");
        close(sockfd);
        return NULL;
    }
    printf("Listening for commands on UDP port 10001...\n");
    // 循环监听并接收指令
    while (1) {
        struct sockaddr_in client_addr;
        socklen_t client_addr_len = sizeof(client_addr);
        char buffer[BUF_SIZE];
        ssize_t len = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0,
            (struct sockaddr*)&client_addr, &client_addr_len);
        if (len < 0) {
            perror("recvfrom error");
            return NULL;
        }
        buffer[len] = '\0';  // 确保字符串以空字符结束

        // 为允许执行的指令创建新线程处理

        struct cmd_args* args = malloc(sizeof(struct cmd_args));
        if (!args) {
            fprintf(stderr, "Memory allocation failed\n");
            return NULL;
        }
        strncpy(args->cmd, buffer, sizeof(args->cmd) - 1);
        args->cmd[sizeof(args->cmd) - 1] = '\0';
        args->client_addr = client_addr;
        args->sockfd = sockfd;
        pthread_t tid;
        if (pthread_create(&tid, NULL, command_handler_thread_func, args) != 0) {
            perror("Failed to create thread for command");
            free(args);
        }
        else {
            pthread_mutex_lock(&thread_mutex);
            add_thread_to_list(tid);
            pthread_mutex_unlock(&thread_mutex);
            pthread_detach(tid);
        }
    }
    // 不会执行到这里，关闭 socket（理论上永不触发）1
    close(sockfd);
    return NULL;
}


#pragma endregion



char myName[64];        // 保存每个CCD的名称Blue1, Blue2, Blue3, Blue4
uint32_t isMaster = 0;  // 默认全部设为0，即slave模式
char binFilename[512];  // 保存输出bin文件的名字

char logFilename[1024]; // 
FILE *fp_log;

static char g_done_str[64] = {0};


uint32_t cStatus = 0;   // 记录当前的工作状态，有且仅有两种可能得状态：
                        // 0: 表示空闲，没有曝光或者读出
                        // 1: 表示接收到ICS的拍摄指令了，正在执行中

int ccd_readout_fpXX( uint32_t *gpio_reg_ctrl, 
                     uint32_t *gpio_reg_status, 
                     uint32_t n_row,
                     uint32_t n_col,
                     uint32_t n_bytes,    // 2 for 16bits unsigned; 4 for 32bits float
                     char *filename );

void *serverForClient(void * arg);
void abortExposure(void *arg);

static int global_abort_exposure = 0;

// —————— 新增：全局配置结构 ——————
typedef struct {
    int   chip_id;
    char  ics_ip[16];
    int   ics_port;
    char  ps_ip[16];
    int   ps_port;
    
} config_t;
static config_t g_cfg;




static void send_done_message(void) {
    fprintf(stderr, "udp%s", g_done_str);

    int sock = socket(AF_INET, SOCK_DGRAM, 0);
    if (sock < 0) {
        perror("socket for done_msg");
        return;
    }

    struct sockaddr_in dst = { 0 };
    dst.sin_family = AF_INET;
    dst.sin_port = htons(g_cfg.ics_port);
    if (inet_aton(g_cfg.ics_ip, &dst.sin_addr) == 0) {
        fprintf(stderr, "invalid ics_ip: %s\n", g_cfg.ics_ip);
        close(sock);
        return;
    }

    char msg[64];
    int len = snprintf(msg, sizeof(msg),
        "get_frame_290:done=%s",
        g_done_str);
    if (len < 0 || len >= (int)sizeof(msg)) {
        close(sock);
        return;
    }

    // ====== 新增：发送 + 重试机制 ======
    const int max_retries = 3;    // 最多重试次数
    const int retry_delay_ms = 200; // 重试间隔 (毫秒)
    int attempt;
    for (attempt = 1; attempt <= max_retries; attempt++) {
        ssize_t ret = sendto(sock, msg, len, 0,
            (struct sockaddr*)&dst, sizeof(dst));
        if (ret < 0) {
            perror("sendto failed");
            if (attempt < max_retries) {
                fprintf(stderr, "Retrying send (%d/%d)...\n", attempt, max_retries);
                fprintf(fp_log, "Retrying send (%d/%d)...\n", attempt, max_retries);
                fflush(fp_log);
                struct timespec ts;
                ts.tv_sec = retry_delay_ms / 1000;
                ts.tv_nsec = (retry_delay_ms % 1000) * 1000000L;
                nanosleep(&ts, NULL);  // 等待指定毫秒再重试
                continue;
            }
            else {
                fprintf(stderr, "All send attempts failed.\n");
                fprintf(fp_log, "All send attempts failed.\n");
                fflush(fp_log);
            }
        }
        else if (ret != len) {
            fprintf(stderr, "Partial send: sent %zd/%d bytes\n", ret, len);
            fprintf(fp_log, "Partial send: sent %zd/%d bytes\n", ret, len);
            fflush(fp_log);
        }
        else {
            fprintf(stderr, "sendto success: %s\n", msg);
            fprintf(fp_log, "sendto success: %s\n", msg);
            fflush(fp_log);
            break; // 成功发送，退出循环
        }
    }

    close(sock);
}
static void send_done_message_ps(void) {
    fprintf(stderr, "udp%s", g_done_str);

    int sock = socket(AF_INET, SOCK_DGRAM, 0);
    if (sock < 0) {
        perror("socket for done_msg");
        return;
    }

    struct sockaddr_in dst = { 0 };
    dst.sin_family = AF_INET;
    dst.sin_port = htons(g_cfg.ps_port);
    if (inet_aton(g_cfg.ps_ip, &dst.sin_addr) == 0) {
        fprintf(stderr, "invalid ps_ip: %s\n", g_cfg.ps_ip);
        close(sock);
        return;
    }

    char msg[64];
    int len = snprintf(msg, sizeof(msg),
        "get_frame_290_ps:done=%s",
        g_done_str);
    if (len < 0 || len >= (int)sizeof(msg)) {
        close(sock);
        return;
    }

    const int max_retries = 3;        // 最多重试次数
    const int retry_delay_ms = 200;   // 重试间隔（毫秒）

    int attempt;
    for (attempt = 1; attempt <= max_retries; attempt++) {
        ssize_t ret = sendto(sock, msg, len, 0,
            (struct sockaddr*)&dst, sizeof(dst));
        if (ret < 0) {
            perror("sendto failed");
            if (attempt < max_retries) {
                fprintf(stderr, "Retrying send (%d/%d)...\n", attempt, max_retries);
                fprintf(fp_log, "Retrying send (%d/%d)...\n", attempt, max_retries);
                fflush(fp_log);
                struct timespec ts;
                ts.tv_sec = retry_delay_ms / 1000;
                ts.tv_nsec = (retry_delay_ms % 1000) * 1000000L;
                nanosleep(&ts, NULL);
                continue;
            }
            else {
                fprintf(stderr, "All send attempts failed.\n");
                fprintf(fp_log, "All send attempts failed.\n");
                fflush(fp_log);
            }
        }
        else if (ret != len) {
            fprintf(stderr, "sendto partial send: %zd/%d bytes\n", ret, len);
            fprintf(fp_log, "sendto partial send: %zd/%d bytes\n", ret, len);
            fflush(fp_log);
        }
        else {
            fprintf(stderr, "sendto success: %s\n", msg);
            fprintf(fp_log, "sendto success: %s\n", msg);
            fflush(fp_log);
            break; // 成功发送，退出重试
        }
    }

    fprintf(stderr, "get_frame_290_ps:done=%s", g_done_str);
    fprintf(fp_log, "get_frame_290_ps:done=%s", g_done_str);
    fflush(fp_log);
    close(sock);
}



int main( int argc, char *argv[] ) {

    if (argc < 3) {
        printf("\n*********************************************\n");
        printf("usage: %s myName logFilename.txt\n", argv[0], argv[1]);
        printf("*********************************************\n");
        return 0;
    }
    strncpy(g_done_str, argv[1], sizeof(g_done_str) - 1);

    sprintf(myName, "%s", argv[1]);
    fprintf(stdout, "# My Name is : %s\n", myName);

    //char fits_file[512]; // 要足够大以容纳完整路径
    //snprintf(fits_file, sizeof(fits_file), "%s/%s_%s.fits",
    //    fits_path, fits_name, argv[1]);

    //fprintf(stderr, "FITS file path: %s\n", fits_file);

    sprintf(logFilename, "data/fits_tmp/%s_log.txt", argv[2]);
    fprintf(stdout, "# log filename: %s\n", logFilename);

    fp_log = fopen(logFilename, "wa");



    int tmp_id = atoi(argv[1]);
    if (tmp_id >= 0 && tmp_id <= 99) {  // 可根据实际约束范围调整
        ccd_id = tmp_id;
        printf("CCD ID set from argument: %d\n", ccd_id);
    }

    strcpy(g_cfg.ics_ip, "192.168.0.233");
    strcpy(g_cfg.ps_ip, ip_table[2]);

    g_cfg.ics_port = port_suffix[1][ccd_id+1];
    g_cfg.ps_port = 10001;


    pthread_mutex_init(&target_mutex, NULL);
    pthread_mutex_init(&thread_mutex, NULL);


    // 创建心跳广播线程和指令监听线程
    pthread_t heartbeat_thread, listener_thread;

    if (pthread_create(&heartbeat_thread, NULL, heartbeat_thread_func, NULL) != 0) {
        perror("Failed to create heartbeat thread");
        return 1;
    }
    pthread_detach(heartbeat_thread);   

    if (pthread_create(&listener_thread, NULL, command_listener_thread_func, NULL) != 0) {
        perror("Failed to create listener thread");
        return 1;
    }
    pthread_detach(listener_thread);  


    //    // 在最开始，先读取配置文件；若失败，则打印并退出
    // if (read_config("/mnt/ics.ini", &g_cfg) != 0) {
    //     fprintf(stderr, "Failed to read config ics.ini: %s\n",
    //         strerror(errno));
    //     return 1;
        
    // }

    // sprintf(binFilename, "data/bin_tmp/%s.bin", argv[1]);
    //    sprintf(binFilename, "/tmp/%s.bin", argv[1]);  // 测试写入用内存模拟的磁盘里

    int fd_gpio = open("/dev/mem", O_RDWR | O_SYNC);

    uint32_t *gpio_flt_ctrl     = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_Float_Point_CTRL);
    uint32_t *gpio_flt_data     = gpio_flt_ctrl + 2;

    uint32_t *gpio_in           = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_GPIO_IN);
    uint32_t *gpio_pl_ps        = gpio_in + 2;

    uint32_t *gpio_out          = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_GPIO_OUT);
    uint32_t *gpio_out2         = gpio_out + 2;

    uint32_t *gpio_spi_ps_ctrl  = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_GPIO_SPI_PS_CTRL);
    uint32_t *gpio_spi_ps_data  = gpio_spi_ps_ctrl + 2;

    uint32_t n_row      = 4700;
    uint32_t n_col      = 1250;
    uint32_t n_bytes    = 2;

    uint32_t base_addr = 0x20000000;    // 512MByte 特殊地址值！！！！
    // int fd = open("/dev/mem", O_RDWR | O_SYNC);
    uint32_t mem_size_in_Bytes = 1264*16*n_bytes;   // the actual number of Bytes of a row (1264 instead of 1250)
    // uint32_t *mem_ptr = mmap(NULL, n_row*1264*16*n_bytes, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, base_addr);
    mem_ptr = mmap(NULL, n_row*1264*16*n_bytes, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, base_addr);

    while(1) {

        // if( getbit(*gpio_in, 13) ){
        if( getbit(*gpio_in, 14) ){
            
            gen_timestamp(timestamp);
            fprintf(stderr, "[%s]> captured PL readout trigger signal\n", timestamp);
            fprintf(fp_log, "[%s]> captured PL readout trigger signal\n", timestamp);
            fflush(fp_log);


            char fits_file[512];
            if (fits_path[0] && fits_name[0]) {
                snprintf(fits_file, sizeof(fits_file),
                    "%s/%s_%s.fits", fits_path, fits_name, argv[2]); // argv[2]=name
            }
            else {
                snprintf(fits_file, sizeof(fits_file),
                    "data/fits_tmp/%s_%ld.fits", argv[2], (long)time(NULL));
            }
            gen_timestamp(timestamp);
            fprintf(stderr, "[%s]> FITS file path: %s\n", fits_file);


            ccd_readout_fpXX( gpio_out, 
                             gpio_in, 
                             n_row,
                             n_col,
                             n_bytes,    // 2 for 16bits unsigned; 4 for 32bits float
                             fits_file );

            printf("> saved %s\n", fits_file);
            fprintf(fp_log, "> saved %s\n", fits_file);
            fflush(fp_log);
        }

//        usleep(1000);   // sleep 1ms
        // usleep(20);   // sleep 10us @20250806
       usleep(100);   // sleep 100us @20250807
    }

    munmap(mem_ptr, n_row*1264*16*n_bytes);

    return 0;
}




int ccd_readout_fpXX( uint32_t *gpio_reg_ctrl, 
                     uint32_t *gpio_reg_status, 
                     uint32_t n_row,
                     uint32_t n_col,
                     uint32_t n_bytes,    // 2 for 16bits unsigned; 4 for 32bits float
                     char *fits_file ){
    gen_timestamp(timestamp);
    fprintf(stderr, "[%s]> entering : %s\n", timestamp, __func__);
    fprintf(fp_log, "[%s]> entering : %s\n", timestamp, __func__);
    fflush(fp_log);

    // 创建FITS文件
    fitsfile_embedded fptr;
    long naxes[2] = {COLS, ROWS};  // [列数, 行数] = [10000, 9400]
    if (fits_create_file_embedded(&fptr, fits_file) != 0) {
        printf("错误: 无法创建FITS文件\n");
        return 1;
    }
    
    // 创建FITS图像并预留头文件空间
    // TODO: 头文件要预留足够的空间，后续更新具体头文件key-value
    if (fits_create_img_embedded(&fptr, FITS_USHORT_IMG, 2, naxes) != 0) {
        printf("错误: 无法创建FITS图像\n");
        fits_close_file_embedded(&fptr);
        return 1;
    }
    gen_timestamp(timestamp);
    fprintf(stderr, "[%s]> created fits", timestamp);


////////////////////////////////////////////////////////////////////////////////////////////    
    // 将*ptr指向PS端DDR存储数据的初始地址
    uint32_t *ptr = mem_ptr;
    uint32_t row_cnt = 0;   // 行计数器

    // 记录读出开始时间
    struct timeval start;
    struct timeval end;
    gettimeofday(&start, NULL);

    // 记录开始时间
    struct timeval start_time, end_time;
    gettimeofday(&start_time, NULL);

    gen_timestamp(timestamp);
    fprintf(stderr, "[%s]> entering writing", timestamp);

    while(1){
        struct timeval whiles,whilee;
        gettimeofday(&whiles, NULL);
        
        // 获取PL端当前行数 - 直接读取gpio_reg_status寄存器的0-12位
        // 标志为htrans_cnt_ps，对应GPIO_STATUS_tri_i的某个位置
        uint32_t pl_row_count = (*gpio_reg_status) & 0x1FFF;  // 13位掩码(0x1FFF)
        
        // 计算落后程度
        int lag = pl_row_count - row_cnt;
        
        if(lag <= 0) {
            // printf("PS超前PL %d行，等待2ms...\n", lag);
            gen_timestamp(timestamp);
            // fprintf(stderr, "[%s]> PS ahead: row_cnt=%u, pl_row_count=%u, lag=%d\n", timestamp, row_cnt, pl_row_count, lag);
            struct timespec ts = {0, 2000000}; // 2ms
            nanosleep(&ts, NULL);
            continue;
        } else {
            // printf("PS落后PL %d行，写入 (row_cnt: %d, PL: %d)\n", lag, row_cnt, pl_row_count);     
            // uint16_t *current_raw_data = ptr + row_cnt * RAW_DATA_PER_READ;
            uint16_t *raw_data = (uint16_t*)ptr + row_cnt * RAW_DATA_PER_READ;
            // if (fits_write_two_rows_embedded(&fptr, row_cnt, raw_data) != 0) {
            //     printf("错误: 写入FITS文件失败, row_cnt: %d\n", row_cnt);
            //     break;
            // }
            fits_write_two_rows_embedded(&fptr, row_cnt, raw_data);
            row_cnt++;
        }
        
        gettimeofday(&whilee, NULL);
        g_time_while_us += (whilee.tv_sec - whiles.tv_sec) * 1e6 + (whilee.tv_usec - whiles.tv_usec);
        
        if(row_cnt >= n_row) break;
    }

    gettimeofday(&end, NULL);
    fprintf(stdout, "FPGA readout used time: %10.6f s\n", time_diff(&start, &end));

    uint32_t rd_is_done = getbit( *gpio_reg_status, BIT_READOUT_DONE );
    
    while( rd_is_done != 1 ){   // check whether readout is over
         rd_is_done = getbit(*gpio_reg_status, BIT_READOUT_DONE);
         if( rd_is_done == 1 ){
            printf("[CCD_READOUT] --> finish!\n");
            break;
        }
        usleep(1000);
    }
    
    // fits写入磁盘
    gen_timestamp(timestamp);
    fprintf(stderr, "[%s]> start closing fit\n", timestamp);
    fits_close_file_embedded(&fptr);
    gen_timestamp(timestamp);
    fprintf(stderr, "[%s]> finished writing FITS file\n", timestamp);

    // 打印统计结果
    fprintf(stderr, "[profiling] header      total = %.3f ms\n", g_time_header_us / 1000.0);
    // fprintf(stderr, "[profiling] wait_ddr    total = %.3f ms\n", g_time_wait_ddr_us / 1000.0);
    // fprintf(stderr, "[profiling] write_total total = %.3f ms\n", g_time_write_total_us / 1000.0);
    // fprintf(stderr, "[profiling] calc_offsets total = %.3f ms\n", g_time_calc_offsets_us / 1000.0);
    // fprintf(stderr, "[profiling] rearrange   total = %.3f ms\n", g_time_rearrange_us / 1000.0);
    // fprintf(stderr, "[profiling] write_top   total = %.3f ms\n", g_time_write_top_us / 1000.0);
    // fprintf(stderr, "[profiling] write_bottom total = %.3f ms\n", g_time_write_bottom_us / 1000.0);
    fprintf(stderr, "[profiling] fflush      total = %.3f ms\n", g_time_fflush_us / 1000.0);
    fprintf(stderr, "[profiling] close       total = %.3f ms\n", g_time_close_us / 1000.0);
    fprintf(stderr, "[profiling] while       total = %.3f ms\n", g_time_while_us / 1000.0);
    // 记录结束时间并计算总时间
    gettimeofday(&end_time, NULL);
    printf("Successfully created FITS file: %s\n", fits_file);
    fprintf(stdout, "Fits finishedi used time: %10.6f s\n", time_diff(&start_time, &end_time));

    send_done_message_ps();
    gen_timestamp(timestamp);
    fprintf(stderr, "[%s]> sent get_frame_done to Master PS\n", timestamp);
    fprintf(fp_log, "[%s]> sent get_frame_done to Master PS\n", timestamp);
    fflush(fp_log);

    usleep(500000);

    send_done_message();
    gen_timestamp(timestamp);
    fprintf(stderr, "[%s]> sent get_frame_done to ICS\n", timestamp);
    fprintf(fp_log, "[%s]> sent get_frame_done to ICS\n", timestamp);
    fflush(fp_log);

    
    DropCaches(3);    //  清空缓存

    gen_timestamp(timestamp);
    fprintf(stderr, "[%s]> leaving : %s\n", timestamp, __func__);
    fprintf(fp_log, "[%s]> leaving : %s\n", timestamp, __func__);
    fflush(fp_log);

    return 0;
}